/*
 * message_queue.c
 *
 *  Created on: 2017年9月18日
 *      Author: shine
 */

#include "message_queue.h"
#include "robotinterface.h"
#include <stdio.h>
#include <stdbool.h>
#include <time.h>
#include <string.h>

// 完全一样
uint8_t robotinterface_get_message_count()
{
	uint8_t m_top = messagequeue_top;
	return  m_top -= messagequeue_bottom;
}

// 完全一样
int robotinterface_get_message_codes(struct message_t *msg,
											int *error_code, int *error_argument)
{
	int ret = 0;
	if (messagequeue_top - messagequeue_bottom)
	{
		msg->timestamp = messagequeue_time[messagequeue_bottom];
		msg->source = messagequeue_joint[messagequeue_bottom];
		*error_code = messagequeue_errorCode[messagequeue_bottom];
		*error_argument = messagequeue_errorArgument[messagequeue_bottom];
		messagequeue_bottom++;
		ret = 1;
	}
	return ret;
}

// 完全一样
int robotinterface_get_message(struct message_t *message)
{
	int ret = 0;
	if (messagequeue_top - messagequeue_bottom) {
		message->timestamp = messagequeue_time[messagequeue_bottom] ;
		message->source = messagequeue_joint[messagequeue_bottom];
		ret = sprintErrorMessage(message->text, messagequeue_errorCode[messagequeue_bottom],
				messagequeue_errorArgument[messagequeue_bottom]);
		messagequeue_bottom++;
	}
	return ret;
}

// 完全一样
bool messagequeue_add_message(time_t time, int8_t jointmode, uint8_t errCode, uint8_t errorArgument)
{
	bool ret;

	if (messagequeue_top - messagequeue_bottom <= 0xFF)
	{
		messagequeue_time[messagequeue_top] = time;
		messagequeue_joint[messagequeue_top] = jointmode;
		messagequeue_errorCode[messagequeue_top] = errCode;
		messagequeue_errorArgument[messagequeue_top++] = errorArgument;
		ret =  true;
	}
	else
	{
		char* s = "messagequeue_add message: warning: queue is full\n";
		fwrite(s, 1, strlen(s), stderr);
		ret =  false;
	}
	return ret;
}
